/* ######################################################################### */
/* \file ObjectFinder.hpp
 * \brief	This file contains the function that
 *
 *
 * PROJECT:   DATMO ROS NODE
 *
 * Copyright (c) 2011 CAR - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodriguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.01
 * \date      2011-06-23
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Gonzalo RODRIGUEZ         2011-06-23 \n
 *      First .
 *
 ######################################################################### */


#include <math.h>
#include <iostream>
#include <set>
/* ------------------------------------------------------------------------- */
/**	\namespace DATMO
 *
 *  \brief	This is the namespace of the DATMO ROS NODE
 */
/* ------------------------------------------------------------------------- */

namespace DATMO
{
/* ------------------------------------------------------------------------- */
/**	\class ObjectFinder
 *
 *  \brief  This is the class of ObjectFinder function.
 *
 *
 */
/* ------------------------------------------------------------------------- */
class cl_ObjectFinder
{
public:
	struct Punto
	{
		int x;
		int y;
		Punto():x(0),y(0){}
		Punto(int rx, int ry):x(rx),y(ry){}
	};
	struct Punto_op
	{
		bool operator()(Punto p1, Punto p2) const
		{
			bool res;
			if(p1.x < p2.x) {res = true;}
			else if (p1.x == p2.x)
			{
				if (p1.y <= p2.y) {res = true;}
				else { res = false;}
			}
			else { res = false;}
			return res;   // so that the set is ordered in descending order...
		}
	};
	typedef std::set<Punto,Punto_op> PuntoSet;

	/* ------------------------------------------------------------------------- */
	/**	\fn public static void ObjectDetection()
	 *
	 *  \brief
	 *
	 *  \param[in]
	 *  \param[in]
	 *
	 *  \param[out] bool
	 *
	 */
	/* ------------------------------------------------------------------------- */
	cl_ObjectFinder(){}

	static DetectedObjects ObjectFinder(Matrix DistanceDiference)
	{
		DynObjConst str_DynObjConst = DynObjConst();
		DetectedObjects str_DetectedObjects = DetectedObjects(DistanceDiference.Nrows(),DistanceDiference.Ncols());
		int n_matrix = DistanceDiference.Nrows();
		int m_matrix = DistanceDiference.Ncols();
		Matrix Objects = Matrix(n_matrix,m_matrix);
		Objects = 0.0; //Intitialize the matrix to zeros
//		int ti_Objects [n_matrix][m_matrix];
		int u1 [] = { 0, 1, 1, 1, 0, -1, -1, -1 };
		int u2 [] = { 1, 1, 0, -1, -1, -1, 0, 1 };
		int label = 0; //label for the objects
		int pos_i, pos_j,pos_i_act,pos_j_act;

//		Punto aux_pos;
//		Punto act_px;
//		PuntoSet ts_OpenPx;
//		PuntoSet ts_ClosedPx;
		std::set<float> OpenPx;
		std::set<float> ClosedPx;
		float aux_pos;
		std::set<int> Objects2eliminate;
		std::set<int> ObjectsLabels;
		set<int>::iterator it;
		int n_open_px = 0;
		int n_pixels = 0;
		bool cond1 = false;
		bool cond2 = false;
		int min_pix = str_DynObjConst.i_min_number_pixel;
		//Fill the objects matrix with zeros
		ROS_INFO("Finder entra");
//		for (int i = 0; i < n_matrix; i++)
//		{
//			for (int j = 0; j < m_matrix; j++)
//			{
//				ti_Objects[i][j] = 0;
//			}
//		}
//		ROS_INFO("Finder 2");
		//Search for objects
		for (int i = 1; i <= n_matrix; i++)
		{
			for (int j = 1; j <= m_matrix; j++)
			{

				//If the difference is not zero (there is a object in this pixel)
				//There is not a previous object defined for this pixel
				//We have found a new object
				if (DistanceDiference(i, j) != 0 && Objects(i,j) == 0)
				{
		//			ROS_INFO("Objeto Nuevo");
					//reset the list for open and closed pixels
					//n_open_px = 0;
					//n_closed_px = 0;
					OpenPx.clear();
					ClosedPx.clear();
					//Surroundings check of the pixel
					for (int k = 0; k < 8; k++)
					{
						pos_i = i + u1[k];
						pos_j = j + u2[k];
						//check if it is between boundaries
						if ((pos_i > 0) && (pos_i <= n_matrix) && (pos_j > 0) && (pos_j <= m_matrix))
						{
							//check if forms part of the object
							if (DistanceDiference(pos_i,pos_j) != 0)
							{
								//add to the open pixels list
//								aux_pos.x = pos_i;
//								aux_pos.y = pos_j;
//								ts_OpenPx.insert(aux_pos);
								aux_pos = (pos_i-1)*m_matrix+pos_j;
								OpenPx.insert(aux_pos);
								n_open_px++;
							}
						}
					}
					aux_pos = (i-1)*m_matrix+j;
					ClosedPx.insert(aux_pos);
					//explore all this object if it has more than one pixel
					if (n_open_px > 0)
					{
						n_pixels = 1;
						label++;
//						ti_Objects[i-1][j-1] = label;
						Objects(i,j) = label;
						while (OpenPx.size() > 0)
						{
							float act_px = *OpenPx.begin();
							pos_j_act = (int)(fmod(act_px,(float)m_matrix));
							pos_i_act = (int)((act_px-pos_j_act)/m_matrix);
							ROS_INFO("actpx = %f,pos_i= %d, pos_j = %d",act_px,pos_i_act,pos_j_act);
							if(pos_j_act==0){pos_j_act=m_matrix;}
	//						ROS_INFO("Mirando en px %d - %d con %d pixeles abiertos",pos_i,pos_j,n_open_px);
							if (DistanceDiference(pos_i_act,pos_j_act) != 0)  //if the difference is not zero the pixel belongs to the object
							{
								n_pixels++;
								Objects(pos_i_act,pos_j_act) = label;
								//ti_Objects[pos_i_act-1][pos_j_act-1] = label;  //we label the pixel as part of the current object
								//look in the surroundings of the pixel in order to introduce more objects in open_px

								for (int k = 0; k < 8; k++)
								{
									pos_j = pos_j_act + u2[k];
									pos_i = pos_i_act + u1[k];
		//							ROS_INFO("Mirando en px %d - %d con %d pixeles abiertos",pos_i,pos_j,n_open_px);
									//pos_i = act_px.x + u1[k];
									//pos_j = act_px.y + u2[k];
									aux_pos = (pos_i-1)*m_matrix+pos_j;
									//find if the pixel between the boundaries
									if ((pos_i > 0) && (pos_i <= n_matrix) && (pos_j > 0) && (pos_j <= m_matrix))
										//find if the pixel is in open or closed list
									{
										cond1 = (OpenPx.count(aux_pos) > 0);
										cond2 = (ClosedPx.count(aux_pos) > 0);

										if (!cond1 && !cond2) //is not in any list
										{
				//							ROS_INFO("Insertando px %d - %d",pos_i,pos_j);
											OpenPx.insert(aux_pos);
											//n_open_px++;
										}
									}
								}
							}
							// add pixel to closed List
							ClosedPx.insert(act_px);
							OpenPx.erase(act_px);
							//n_open_px--;
						}
						if (n_pixels < min_pix)
						{
							Objects2eliminate.insert(label);
						}
						else
						{
							ObjectsLabels.insert(label);
						}

					}
				}
			}
		}
	//	ROS_INFO("antes de eliminar");
		for ( it = Objects2eliminate.begin(); it != Objects2eliminate.end(); it++)
		{
			for (int i = 1; i <= n_matrix; i++)
			{
				for (int j = 1; j <= m_matrix; j++)
				{
					if (Objects(i,j) == *it)
						Objects(i,j) = 0;
				}
			}
			label--;
		}
//		for (int i = 0; i < n_matrix; i++)
//		{
//			for (int j = 0; j < m_matrix; j++)
//			{
//				str_DetectedObjects.m_Objects(i+1,j+1) = ti_Objects[i][j];
//			}
//		}
		str_DetectedObjects.m_Objects = Objects;
		ROS_INFO("Fin finder con %d objetos encontrados",label);
		str_DetectedObjects.i_NumberOfObjects = label;
		str_DetectedObjects.s_labels = ObjectsLabels;
		return str_DetectedObjects;
	}
};
}




